#include <chrono> // 包含时间库
#include <future> // 包含异步任务库
#include <memory> // 包含智能指针库
#include <string> // 包含字符串库
#include <thread> // 包含线程库

#include "lifecycle_msgs/msg/state.hpp"        // 包含生命周期消息中的状态消息
#include "lifecycle_msgs/msg/transition.hpp"   // 包含生命周期消息中的转换消息
#include "lifecycle_msgs/srv/change_state.hpp" // 包含生命周期服务中的更改状态服务
#include "lifecycle_msgs/srv/get_state.hpp"    // 包含生命周期服务中的获取状态服务

#include "rclcpp/rclcpp.hpp" // 包含ROS 2客户端库

using namespace std::chrono_literals; // 使用时间字面量

// 要处理的节点
static constexpr char const *lifecycle_node = "basic_cmd";

// 每个生命周期节点都有多个服务
// 按照惯例，我们使用 <节点名>/<服务名> 的格式。
// 在这个示例中，我们使用 get_state 和 change_state
// 因此两个服务主题是：
// lc_talker/get_state
// lc_talker/change_state
static constexpr char const *node_get_state_topic = "basic_cmd/get_state";
static constexpr char const *node_change_state_topic = "basic_cmd/change_state";

// 等待结果函数模板
template <typename FutureT, typename WaitTimeT>
std::future_status wait_for_result(FutureT &future,        // 异步任务
                                   WaitTimeT time_to_wait) // 等待时间
{
    auto end = std::chrono::steady_clock::now() + time_to_wait; // 计算结束时间
    std::chrono::milliseconds wait_period(100);                 // 每次等待100毫秒
    std::future_status status = std::future_status::timeout;    // 初始状态设为超时
    do
    {
        auto now = std::chrono::steady_clock::now(); // 获取当前时间
        auto time_left = end - now;                  // 计算剩余时间
        if (time_left <= std::chrono::seconds(0))
        {
            break;
        } // 如果剩余时间小于等于0，跳出循环
        status = future.wait_for((time_left < wait_period) ? time_left : wait_period); // 等待结果
    } while (rclcpp::ok() && status != std::future_status::ready); // 如果ROS正常运行且状态不是就绪，继续等待
    return status; // 返回最终状态
}

// 生命周期服务客户端类
class LifecycleServiceClient : public rclcpp::Node
{
public:
    explicit LifecycleServiceClient(const std::string &node_name)
        : Node(node_name) // 初始化父类 Node
    {
    }

    // 初始化函数
    void init()
    {
        // 每个生命周期节点自动生成几个服务
        // 允许外部与这些节点进行交互
        // 主要的两个服务是 GetState 和 ChangeState
        client_get_state_ = this->create_client<lifecycle_msgs::srv::GetState>(
            node_get_state_topic); // 创建获取状态服务客户端
        client_change_state_ = this->create_client<lifecycle_msgs::srv::ChangeState>(
            node_change_state_topic); // 创建更改状态服务客户端
    }

    // 请求节点的当前状态
    /**
     * 在这个函数中，我们发送服务请求
     * 询问节点 lc_talker 的当前状态。
     * 如果在给定的超时时间内没有返回，
     * 我们返回节点的未知状态，否则返回当前状态。
     * \param time_out 等待响应的超时时间（秒）
     */
    unsigned int get_state(std::chrono::seconds time_out = 3s)
    {
        auto request = std::make_shared<lifecycle_msgs::srv::GetState::Request>(); // 创建请求

        if (!client_get_state_->wait_for_service(time_out))
        { // 等待服务
            RCLCPP_ERROR(
                get_logger(),
                "Service %s is not available.",
                client_get_state_->get_service_name());               // 打印错误信息
            return lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN; // 返回未知状态
        }

        // 发送服务请求以询问 lc_talker 节点的当前状态
        /*  future : 从返回结果中提取 future 对象
            async_send_request(request)的返回类型通常是 rclcpp::Client<T>::SharedResponseFuture,包含一个 .future 字段，该字段实际上是一个std::future对象
            使用.future 可以直接获取 std::future 对象，表示 `请求的最终结果 `(即服务的响应)

            .share
            std::future 的 share()方法会将 std::future 转换为 std::shared_future, 允许多个对象或线程同时访问相同的结果
            std::future 本身是不可复制的，只能移动或通过单一线程访问。而 std::shared_future 可以在多个地方共享这个结果，可以在多个调用者间重复使用，
            不必在多个位置重新发起请求。
            .share()转换后，future_result变成了std::shared_future 类型，允许在多个地方等待或读取这个请求的结果。
        */
        auto future_result = client_get_state_->async_send_request(request).future.share();

        // 等待节点的响应
        // 如果请求超时，返回未知状态
        auto future_status = wait_for_result(future_result, time_out);

        if (future_status != std::future_status::ready)
        { // 如果状态不是就绪
            RCLCPP_ERROR(
                get_logger(), "Server time out while getting current state for node %s", lifecycle_node);
            return lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN; // 返回未知状态
        }

        // 成功接收到响应，打印当前状态
        if (future_result.get())
        {
            RCLCPP_INFO(
                get_logger(), "Node %s has current state %s.",
                lifecycle_node, future_result.get()->current_state.label.c_str()); // 打印状态信息
            return future_result.get()->current_state.id;                          // 返回当前状态ID
        }
        else
        {
            RCLCPP_ERROR(
                get_logger(), "Failed to get current state for node %s", lifecycle_node); // 打印错误信息
            return lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN;                     // 返回未知状态
        }
    }

    // 调用转换函数
    /**
     * 我们发送一个服务请求，表示
     * 我们希望调用 id 为 "transition" 的转换。
     * 默认情况下，这些转换是
     * - configure（配置）
     * - activate（激活）
     * - cleanup（清理）
     * - shutdown（关闭）
     * \param transition 表示要调用的转换的ID
     * \param time_out 等待响应的超时时间（秒）
     */
    bool change_state(std::uint8_t transition, std::chrono::seconds time_out = 3s)
    {
        auto request = std::make_shared<lifecycle_msgs::srv::ChangeState::Request>(); // 创建请求
        request->transition.id = transition;                                          // 设置转换ID

        if (!client_change_state_->wait_for_service(time_out))
        { // 等待服务
            RCLCPP_ERROR(
                get_logger(),
                "Service %s is not available.",
                client_change_state_->get_service_name()); // 打印错误信息
            return false;
        }

        // 发送请求以调用转换
        auto future_result = client_change_state_->async_send_request(request).future.share();

        // 等待节点的响应
        // 如果请求超时，返回失败
        auto future_status = wait_for_result(future_result, time_out);

        if (future_status != std::future_status::ready)
        { // 如果状态不是就绪
            RCLCPP_ERROR(
                get_logger(), "Server time out while getting current state for node %s", lifecycle_node);
            return false;
        }

        // 成功接收到响应，打印转换成功信息
        if (future_result.get()->success)
        {
            RCLCPP_INFO(
                get_logger(), "Transition %d successfully triggered.", static_cast<int>(transition)); // 打印成功信息
            return true;
        }
        else
        {
            RCLCPP_WARN(
                get_logger(), "Failed to trigger transition %u", static_cast<unsigned int>(transition)); // 打印警告信息
            return false;
        }
    }

private:
    std::shared_ptr<rclcpp::Client<lifecycle_msgs::srv::GetState>> client_get_state_;       // 获取状态服务客户端
    std::shared_ptr<rclcpp::Client<lifecycle_msgs::srv::ChangeState>> client_change_state_; // 更改状态服务客户端
};

/**
 * 这是一个小的独立脚本，
 * 触发节点的默认生命周期。
 * 它从 configure 开始，接着是 activate、
 * deactivate，再次 activate、deactivate，
 * 然后是 cleanup，最后是 shutdown。
 */
void callee_script(std::shared_ptr<LifecycleServiceClient> lc_client)
{
    rclcpp::WallRate time_between_state_changes(0.1); // 状态变化之间的时间间隔为0.1秒

    using Transition = lifecycle_msgs::msg::Transition; // 使用生命周期消息中的 Transition

    // configure
    {
        if (!lc_client->change_state(Transition::TRANSITION_CONFIGURE))
        {           // 触发 configure 转换
            return; // 如果失败则退出
        }
        if (!lc_client->get_state())
        {           // 获取当前状态
            return; // 如果获取失败则退出
        }
    }

    // activate
    {
        time_between_state_changes.sleep(); // 等待
        if (!rclcpp::ok())
        { // 如果 ROS 2 已关闭，退出
            return;
        }
        if (!lc_client->change_state(Transition::TRANSITION_ACTIVATE))
        {           // 触发 activate 转换
            return; // 如果失败则退出
        }
        if (!lc_client->get_state())
        {           // 获取当前状态
            return; // 如果获取失败则退出
        }
    }

    {
        for (int i = 0; i < 20; i++)
        {
            time_between_state_changes.sleep(); // 等待
        }
    }

    // 再次停用
    {
        time_between_state_changes.sleep(); // 等待
        if (!rclcpp::ok())
        { // 如果 ROS 2 已关闭，退出
            return;
        }
        if (!lc_client->change_state(Transition::TRANSITION_DEACTIVATE))
        {           // 触发 deactivate 转换
            return; // 如果失败则退出
        }
        if (!lc_client->get_state())
        {           // 获取当前状态
            return; // 如果获取失败则退出
        }
    }

    // 进行清理
    {
        time_between_state_changes.sleep(); // 等待
        if (!rclcpp::ok())
        { // 如果 ROS 2 已关闭，退出
            return;
        }
        if (!lc_client->change_state(Transition::TRANSITION_CLEANUP))
        {           // 触发 cleanup 转换
            return; // 如果失败则退出
        }
        if (!lc_client->get_state())
        {           // 获取当前状态
            return; // 如果获取失败则退出
        }
    }

    // 最后关闭
    // 注意：在调用 shutdown 转换ID 时，我们必须准确。
    // 我们当前处于未配置状态，因此必须调用
    // TRANSITION_UNCONFIGURED_SHUTDOWN
    {
        time_between_state_changes.sleep(); // 等待
        if (!rclcpp::ok())
        { // 如果 ROS 2 已关闭，退出
            return;
        }
        if (!lc_client->change_state(Transition::TRANSITION_UNCONFIGURED_SHUTDOWN))
        {           // 触发 shutdown 转换
            return; // 如果失败则退出
        }
        if (!lc_client->get_state())
        {           // 获取当前状态
            return; // 如果获取失败则退出
        }
    }
}

void wake_executor(std::shared_future<void> future, rclcpp::executors::SingleThreadedExecutor &exec)
{
    future.wait(); // 等待脚本完成
    // 当脚本完成时唤醒执行器
    // https://github.com/ros2/rclcpp/issues/1916
    exec.cancel(); // 取消执行器
}

int main(int argc, char **argv)
{
    // 强制刷新 stdout 缓冲区。
    // 确保在启动文件中同时执行时所有打印输出的正确同步。
    // 通过禁用stdout缓冲区来确保输出可以立即显示，这样在多线程或多进程环境中，输出不会被延迟
    setvbuf(stdout, NULL, _IONBF, BUFSIZ);

    rclcpp::init(argc, argv); // 初始化 ROS 2

    // 创建了一个共享指针 lc_client ，指向 LifecycleServiceClient 对象，并指定其节点名称为 lc_client
    // LifecycleServiceClient 用户自定义类，用来管理服务的生命周期
    auto lc_client = std::make_shared<LifecycleServiceClient>("lc_client"); // 创建 LifecycleServiceClient 节点
    lc_client->init();                                                      // 初始化节点

    // 创建执行器
    // 创建一个 SingleThreadedExecutor 执行器，该执行器用于管理和执行节点的回调函数，
    // 单线程执行器表示节点内的回调将在同一个线程中按顺序执行
    rclcpp::executors::SingleThreadedExecutor exe; // 创建单线程执行器
    // 将lc_client节点添加到exe执行器中，使其可以被执行器调度和管理
    exe.add_node(lc_client); // 将节点添加到执行器

    // 启动并行脚本
    // 使用std::async启动一个异步任务 callee_script,并将lc_client作为参数传递给它。
    // 这个任务可能是一个自定义函数 callee_script,用于处理节点生命周期的某些任务。
    // script 是一个 std::shared_future ，可以用于同步和获取任务的完成状态
    std::shared_future<void> script = std::async(
        std::launch::async,
        callee_script,
        lc_client // 启动脚本
    );

    // 启动唤醒执行器任务
    // 使用 std::async 启动另一个异步任务 wake_executor ，将script和exe(以引用形式)传递给它。
    // 这段代码的目的是通过 wake_executor 监控 script 的执行状态，并在某些条件下唤醒执行器 exe
    auto wake_exec = std::async(
        std::launch::async,
        wake_executor,
        script,
        std::ref(exe) // 启动唤醒执行器
    );

    // 这里使用exe的spin_until_future_complete 方法来运行执行器，直到script执行完成
    // 这种方式可以让执行器在异步任务 script 运行的过程中处理任何回调，并在 script 结束时自动停止执行器
    exe.spin_until_future_complete(script); // 执行直到脚本完成

    rclcpp::shutdown(); // 关闭 ROS 2

    return 0;
}